//vel2pwm_rated节点，订阅/cmd_vel，定时发布/pwm。
//有用的数据仅为x轴速度和z轴角速度
// 打角115为正，160左40度，70右40度
// 速度1520不动，1525为0.2但有响声，1530为0.6，1540为0.7，1550为1.0，1560可到1.8左右不过加速还要一段距离，1565为2左右

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

double speed;
double angle;

void TwistCallback(const geometry_msgs::Twist& twist)
{
    angle=twist.angular.z;
    speed=twist.linear.x;
}
//2222222#
double speedcontrol2(double angle)
{
    double speedpwm;
 //   if(angle>=0)speedpwm=1570-25*angle;
 //   else    speedpwm=1570+25*angle;
    if(angle >= 0.7)
    {
        speedpwm = 1575;//1563//1568//63
    }
    else if(angle <= -0.7)
    {
        speedpwm = 1575;//1563//1568//63
    }
    else if(angle >= 0.35)
    {
        //speedpwm = 1570;//1568
        speedpwm=1585-25*angle;//speedpwm=1582-25*angle//65
    }
    else if(angle <= -0.35)
    {
        //speedpwm = 1570;//1568
        speedpwm=1585-25*angle;//speedpwm=1582+25*angle//65
    }
    else if(angle >= 0.15)
    {
        //speedpwm = 1578;//1574
        speedpwm=1590-25*angle;//speedpwm=1588-25*angle//70
    }
    else if(angle <= -0.15)
    {
        //speedpwm = 1578;//1574
        speedpwm=1590-25*angle;//speedpwm=1588+25*angle//70
    }
    else 
    {
        speedpwm = 1593;//1580//1590//75
    }
    
    return speedpwm;

}

//1111111#
double speedcontrol(double angle)
{
    double speedpwm;
 //   if(angle>=0)speedpwm=1570-25*angle;
 //   else    speedpwm=1570+25*angle;
    if(angle >= 0.7)
    {
        speedpwm = 1562;//1563//1568//63
    }
    else if(angle <= -0.7)
    {
        speedpwm = 1562;//1563//1568//63
    }
    else if(angle >= 0.35)
    {
        //speedpwm = 1570;//1568
        speedpwm=1568-25*angle;//speedpwm=1582-25*angle//65
    }
    else if(angle <= -0.35)
    {
        //speedpwm = 1570;//1568
        speedpwm=1568+25*angle;//speedpwm=1582+25*angle//65
    }
    else if(angle >= 0.15)
    {
        //speedpwm = 1578;//1574
        speedpwm=1570-25*angle;//speedpwm=1588-25*angle//70
    }
    else if(angle <= -0.15)
    {
        //speedpwm = 1578;//1574
        speedpwm=1570+25*angle;//speedpwm=1588+25*angle//70
    }
    else 
    {
        speedpwm = 1570;//1580//1590//75
    }
    
    return speedpwm;

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "vel2pwm");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/cmd_vel",1,TwistCallback);
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>( "/pwm", 1 );
    geometry_msgs::Twist pwm;
    ROS_INFO("vel2pwm_rated start successfully.");
    // double t0=ros::Time::now().toSec();
    ros::Rate r(16);
    while(ros::ok())
    {
        ros::spinOnce();
        if(speed==1000)
        {
            pwm.linear.x=1568;
            //pwm.angular.z=150;
        }
        else if(speed==2000)
        {
            pwm.linear.x=speedcontrol2(angle);
            //pwm.angular.z=150;
        }
        else if(speed!=0)
        {
            pwm.linear.x=speedcontrol(angle);
        }
        else
        {
            pwm.linear.x=1490;
        }

        if(pwm.linear.x>1585)
        {
            if(angle >= 0.7) pwm.angular.z = 150; //限幅左右40度
            else if(angle <= -0.7) pwm.angular.z = 75;
            else pwm.angular.z=angle*58+113; //*180/pi*45/40，即弧度制转角度制再转pwm

        }
        else
        {
            if(angle >= 0.7) pwm.angular.z = 150; //限幅左右40度
            else if(angle <= -0.7) pwm.angular.z = 75;
            else pwm.angular.z=angle*60+113; //*180/pi*45/40，即弧度制转角度制再转pwm
        }


        

        
        pub.publish(pwm);
        r.sleep();
    }
    return 0;
}
